#!/usr/bin/env python3
# coding=utf-8

import rospy
from geometry_msgs.msg import Twist, TwistStamped
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import PointStamped
from mavros_msgs.srv import *
from mavros_msgs.msg import State
import math
import sys, select, termios, tty
import cap


# 全局变量用于存储速度信息
current_velocity = TwistStamped()
vx=0
vy=0
vz=0
#定义无人机惯导坐标
x=0
y=0
current_height = 0
#无人机位姿信息
sita = 0.0  # 朝向
z = 0
w = 0
zf = 1
#定义时间变量
last_time = None

def pose_cb(m):
    global sita
    global z
    global w
    global zf
    global current_height
    # z = m.pose.orientation.z
    # w = m.pose.orientation.w
    # if z * w > 0:
    #     zf = 1
    # else:
    #     zf = -1
    # sita = 2 * math.acos(w) * 180 / math.pi
    current_height = m.pose.position.z
def velocity_cb(msg):
    global current_velocity
    global vx
    global vy
    global vz
    current_velocity = msg
    # 提取x, y, z轴速度
    vx = current_velocity.twist.linear.x
    vy = current_velocity.twist.linear.y
    vz = current_velocity.twist.linear.z

    if abs(vx)<=0.01:
        vx=0
    if abs(vy)<=0.03:
        vy=0
    # print(f"Velocity - x: {vx}, y: {vy}, z: {vz}")

def Coordinate_calculation(current_time):#坐标解算函数
    global x
    global y
    global z
    global last_time
    # 时间差（秒）
    if last_time is not None:
        dt = (current_time - last_time).to_sec()
        #计算三轴方向上坐标增量
        d_x = vx*dt
        d_y = vy*dt
        # d_z = vz*0.1
        #更新坐标
        x=x+d_x
        y=y+d_y
    #记录时间
    last_time = current_time  # 更新上次时间


if __name__ == "__main__":
    settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('INS_node')#创建惯性导航节点
    # 初始化发布者
    position_pub = rospy.Publisher('computed_position', PointStamped, queue_size=10)
    rospy.Subscriber('mavros/local_position/velocity_local', TwistStamped, velocity_cb)#订阅无人机速度
    rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_cb)#订阅无人机位姿

    rate = rospy.Rate(100)  # 100 Hz, 即每0.01秒调用一次
    # current_height=0#校正海压令当前高度为0

    while not rospy.is_shutdown():
        current_time = rospy.Time.now()
        Coordinate_calculation(current_time)#坐标解算
        #发布坐标话题
        # 创建 PointStamped 消息
        position_msg = PointStamped()
        position_msg.header.stamp = rospy.Time.now()  # 设置时间戳
        position_msg.header.frame_id = "local_frame"  # 可以自定义坐标系名称
        position_msg.point.x = x
        position_msg.point.y = y
        position_msg.point.z = current_height-10  # 如果需要，也可以使用高度数据
        position_pub.publish(position_msg)

        print(x,y,current_height,end='\r')
        rate.sleep()  # 控制循环频率为10 Hz，即每0.1秒执行一次